#pragma once

#include <typedef.h>

class PID
{
public:
	PID();

	~PID();

	void ParamsUpdate(Eigen::Vector3d p, Eigen::Vector3d i, Eigen::Vector3d d, Eigen::Vector3d ff);

	Eigen::Vector3d Control(double dt, Eigen::Vector3d setpoint, Eigen::Vector3d actual, Eigen::Vector3d integralLimitMin = Eigen::Vector3d().setZero(), Eigen::Vector3d integralLimitMax = Eigen::Vector3d().setZero());

	void ResetIntegral(void);

private:
	Eigen::Vector3d m_GainP;  // 比例
	Eigen::Vector3d m_GainI;  // 积分
	Eigen::Vector3d m_GainD;  // 微分
	Eigen::Vector3d m_GainFF; // 前馈

	Eigen::Vector3d m_ErrCurr;	// 当前误差
	Eigen::Vector3d m_ErrPrev;	// 上一次误差
	Eigen::Vector3d m_Integral; // 积分
};
